#ifndef KF_H_
#define KF_H_
#include "hal_data.h"

//定义陀螺仪的初始方差
#define GYRO_VAR 1.0f
//定义加速度计的初始方差
#define ACC_VAR 1.0f
//定义PI的值
#define IMU_M_PI 3.1415926f
//定义加速度卡尔曼滤波的权重值
#define ACC_KF_WEIGHT 40000.0f
//定义时间间隔，单位为秒
#define TIME_INTERVAL 0.005f
//定义四元数差量反馈时的权重值
#define QUAT_KF_WEIGHT 1.0f
//定义角速度计的阈值，单位为度
#define GYRO_THRESHOLD 0.3f


//定义卡尔曼滤波器结构体
typedef struct {
    float q; //过程噪声协方差
    float r; //测量噪声协方差
    float x; //估计值
    float p; //估计误差协方差
    float k; //卡尔曼增益
} KalmanFilter;

//初始化卡尔曼滤波器
void KF_Init(float *acc_init_value);
//卡尔曼滤波算法
void KF_Update(float *measured_value,float *result_value);

#endif /* KF_H_ */
